package sdp.server.strategy;
/*
import javax.swing.JButton;
import javax.swing.JSlider;

import sdp.server.BallState;
import sdp.server.PcServer;
import sdp.server.gui.ControlPanel;
import sdp.server.gui.ImageViewer;
import sdp.server.math.Line;
import sdp.server.math.MathUtils;
import sdp.server.math.Vector2;
*/

public class MilestoneFourStrategy {
	/*
	private static final int BALL_VELOCITY_HISTORY_SIZE = 16;
	
	private ControlPanel panel         = null;
	private boolean      running       = false;
	private boolean      ballMoving    = false;
	private boolean      doKickBall    = false;
	
	private double       moveThreshold = 0.03;
	//private double       sigmoidK      = -32;
	private Vector2      origBallPos;
	
	private Vector2[]    ballVelHistory  = new Vector2[ BALL_VELOCITY_HISTORY_SIZE ];
	private int          currentVelIndex = 0;
	private ImageViewer  viewer;
	private long         startKickingAt  = -1;
	private Vector2      previousDestination = new Vector2(100,100);
	private double       moveDirection   = Double.NaN;
			
	public MilestoneFourStrategy( PcServer server ) {
		super( server );
		ourSide = -1.0;
	}
	
	public void reset() {
		for( int i = 0 ; i < ballVelHistory.length ; ++ i ) 
			ballVelHistory[ i ] = Vector2.ZERO;
		
		doKickBall      = ballMoving = running = false;
		origBallPos     = null;
		currentVelIndex = 0;
	}
	
	public void prestart() {
		ctrl.setGoalPosition( new Vector2( ctrl.getRobotGoalPoint().getX(), 0 ) );
		ctrl.setGoalRotation( MathUtils.capAngle( Math.atan2( 0, - ourSide ) ) );
		ctrl.setCurrentSpeed( 1.0 );
		ctrl.setStationary( false );
		ctrl.setStopAtGoal( false );
		ctrl.setWallAvoidance( false );
	}
	
	@Override
	public void enable() {
		super.enable();
		reset();
		viewer = server.getGui().getImageViewer();
		createPanel();
	}
	
	@Override
	public void disable() {
		panel.dispose();
		super.disable();
	}
	
	public static double sigmoid( double x, double k ) {
		double sigX;
		
		if( x < 0 ) {
			x    = - x;
			sigX = - 1.0;
		} else {
			sigX = 1.0;
		}
		
		if( x > 1.0 )
			x = 1.0;
				
		return sigX * k * x / ( k - x + 1 );
	}
	
	@Override
	public void update() {		
		if( running ) {
			BallState ball       = world.getBallState();
			Vector2   ballPos    = ball.getPosition();
			Vector2   ballVel    = ball.getLinearVelocity();
			
			if( origBallPos == null )
				origBallPos = world.getBallState().getPosition();
			
			double ballChange = ballPos.minus( origBallPos ).computeNorm();
			
			
			
			if( !ballMoving ) {
				ballMoving = ballChange >= moveThreshold;				
				if( ballMoving ) {
					System.out.println("MilestoneFourStrategy: update(): Ball started moving (" + String.format("%1.4f", ballChange) + ").");
					ballVelHistory[ currentVelIndex ++  ] = ballPos.minus( origBallPos ).times( 1.0 / world.getTimeStep() );
					startKickingAt = System.currentTimeMillis() + 3000;
					ctrl.setStationary( false );
				}
			} else {
				if( !doKickBall ) {
					ctrl.setStationary( false );
					Vector2 robPos = ctrl.getCurrentPosition();
					double  robRot = ctrl.getCurrentRotation();
					
					if( ballVel.computeNorm() > 0.2 ) {
						ballVelHistory[ currentVelIndex ] = ballVel;
						currentVelIndex = ( currentVelIndex + 1 ) % ballVelHistory.length;
					}
					
					for( int i = 0 ; i < ballVelHistory.length ; ++ i ) {
						viewer.drawLine( ballPos, ballPos.plus( ballVelHistory[ i ] ), 255, 0, 0 );
					}
										
					ballVel = Vector2.ZERO;
					for( Vector2 v : ballVelHistory )
						ballVel = ballVel.plus( v );
					
					if( System.currentTimeMillis() > startKickingAt ) {
						doKickBall = true;
						ballChange = 1.0;
					}
					
					ballVel = ballVel.computeUnit();
					
					Line ballLine   = Line.fromPointAndUnit( ballPos, ballVel );
					Line ballNormal = ballLine.getNormal( robPos );
					viewer.drawLine( ballLine.getOrigin(), ballLine.pointAt(10.0), 255, 255, 0);
					
					double t       = ballLine.intersect( ballNormal );
					viewer.drawCircle( ballLine.pointAt( t ), 0.015, 255, 0, 0);
					
					double dist    = ballLine.distanceTo( robPos );
					t += 0.05;
					//double maxDist = 0.25;
					//double prop    = -0.05 + 1.05 * (sigmoid( dist / maxDist * 2.0 - 1.0, sigmoidK ) / 2.0 + 0.5);
					//t -=  * t;
					
					if( Double.isNaN(moveDirection) ) {
						boolean goBackwards = MathUtils.angleDiff( ballNormal.getDirection().computeAngle(), robRot ) >= Math.PI / 2.0;
						moveDirection = goBackwards ? -1.0 : 1.0;
						System.out.println("MilestoneFourStrategy: update(): Going backwards: " + goBackwards );
					}
					
					Vector2 dest = ballLine.pointAt( t ).plus( ballNormal.getDirection().times( - ctrl.getGoalPointOffset() * moveDirection ) );
					
					if( previousDestination.minus( dest ).computeNorm() >= ctrl.getGoalPointOffset() * 2.0 ) {
						ctrl.setGoalPosition( dest );
						ctrl.setGoalRotation( ballPos.minus( dest ).computeAngle() );
						ctrl.setCurrentSpeed( 1.0 );
						
						ctrl.setStopAtGoal( true );
						ctrl.setWallAvoidance( false );
												
						ctrl.setGoBackwards( false );
						
						System.out.println("MilestoneFourStrategy: update(): New intercept point.");
						
						previousDestination = dest;
					}
					
					viewer.drawCircle( previousDestination, 0.015, 0, 255, 0);
				}
				
				if( doKickBall  ) {
					if( ballChange > 0.03 ) {
						Vector2 robPos = ctrl.getCurrentPosition();
						origBallPos = ballPos;
						clearQueue();
						if( Math.abs(Math.abs( ballPos.getY() ) - ctrl.getVirtualPitchSize().getY() / 2.0) < 0.05 ) {
							ctrl.setWallAvoidance( false );
							ctrl.setGoalPosition( ballPos.plus( ballPos.minus(robPos).computeUnit().times( 0.1 ) ) );
							ctrl.setGoalRotation( Math.atan2( 0.0, -ourSide ) );
							ctrl.setCurrentSpeed( 1.0 );
							ctrl.setStationary( false );
							ctrl.setWallAvoidance( false );
							ctrl.setGoBackwards( false );
							
						} else if( getGoalCount() > 3 || getGoalCount() == 0 ) {
							ctrl.setWallAvoidance( true  );
							ctrl.setGoBackwards(false );
							ctrl.setStationary( false );
							planKick( null, 0.8, 0.16, true, false );
						}
					}
					
					super.update();
				}
				
				if( shouldKick() )
					ctrl.kick();
			}
		} else
			ctrl.setStationary( true );
	}
	
	public void createPanel() {
		panel = new ControlPanel();
		panel.setTitle( "Strategy Controls" );
		
		panel.addSlider( "Our Side", ourSide < 0 ? 0 : 1, 0, 1, new ControlPanel.SliderCallback() {
			@Override public void valueChanged( JSlider slider, int newValue ) {
				if( newValue == 0 )
					ourSide = - 1.0;
				else
					ourSide = 1.0;
			}
		});
		
		panel.getSlider("Our Side").setSnapToTicks( true );
		
		panel.addButton("Intercept", new ControlPanel.ButtonCallback() {
			@Override public void buttonClicked( JButton button ) {
				running = true;
			}
		});
		
		panel.addButton("Do Nothing", new ControlPanel.ButtonCallback() {
			@Override public void buttonClicked( JButton button ) {
				reset();
			}
		});
		
		panel.addButton( "Manual Start", new ControlPanel.ButtonCallback() {
			@Override public void buttonClicked(JButton button) {
				running = true;
				prestart();
			}
		});
	}
	*/
}
